# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import sviewgui.sview as sv
import pandas as pd
import sys
import numpy as np
import scipy as sy
from scipy.optimize import root,fsolve
import pandas as pd
import sympy
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
from sympy import lambdify
class 曲柄连杆测试():
    def __init__(self,rc1 = 0.2,l = 0.4,l2 = 1.2,rc2 = 0.6,rp2 = 0.3,g = 10,m1 = 1.2, \
                 m2 = 3.6,m3 = 6,fx2 = 10,fy2=10,fx3 = 20,mm1 = 25,x = 5,y=10,j1=0.01,j2=0.045):
        self.rc1 = rc1
        self.l = l
        self.l2 = l2
        self.rc2 = rc2
        self.rp2 = rp2
        self.g = g
        self.m1 = m1
        self.m2 = m2
        self.m3 = m3
        self.fx2 = fx2
        self.fy2 = fy2
        self.fx3 = fx3
        self.mm1 =mm1
        self.x = x
        self.y = y
        self.j1=j1
        self.j2=j2
        self.曲柄长度=self.l
        self.连杆长度=self.l2
        self.thea=0
        self.初始=[0,0]
        self.角速度=55/60*2*np.pi
        self.res=self.质心雅可比改1()
        self.力的雅克比1=self.力的雅克比()
        return
    def 力矩对比计算(self,x,y):
        力矩对比计算=self.mm1+self.fx2*self.l*np.sin(x)+self.fy2*self.l*np.cos(y)-self.fx3*self.l*np.sin(x) \
               -((self.m1*self.rc1+self.m2*self.l+self.m3*self.l)*self.g*np.cos(x))+ \
               ((self.fx2*self.rp2*np.sin(y)+self.fy2*self.rp2*np.cos(y)-self.fx3*self.l2*np.sin(y)+ \
            ((self.m2*self.rc2+self.m3*self.l2)*self.g*np.cos(y)))* \
               (self.l*np.cos(x)/(self.l2*np.cos(y))))
        return 力矩对比计算
    def 质心复数雅可比矩阵计算(self):
        a, b, c, x, y = sympy.symbols("a b c x y", real=True)
        a = (self.rc1 * sympy.cos(x) + self.rc1 * sympy.sin(x) * 1j)
        b = (self.l * sympy.cos(x) + self.l * sympy.sin(x) * sympy.I) + (self.rc2 * sympy.cos(y) + sympy.I * self.rc2 * sympy.sin(y))
        c = (self.l * sympy.cos(x) + self.l * sympy.sin(x) * sympy.I) + (self.l2 * sympy.cos(y) + self.l2 * sympy.sin(y) * sympy.I)
        funcs = sympy.Matrix([a, b, c])
        args = sympy.Matrix([x, y])
        res = funcs.jacobian(args)
        return res
    def 质心雅克比(self,x,y):
        return np.array([[-0.2*np.sin(x) + 0.2j*np.cos(x), 0],  \
                         [-0.4*np.sin(x) + 0.4j*np.cos(x), -0.6*np.sin(y) + 0.6j*np.cos(y)],  \
                         [-0.4*np.sin(x) + 0.4j*np.cos(x), -1.2*np.sin(y) + 1.2j*np.cos(y)]])
    def 外力雅可比(self):
        d, e ,x,y= sympy.symbols("d e x y", real=True)
        d = (self.l * sympy.cos(x) + self.l * sympy.sin(x) * sympy.I) + (self.rp2 * sympy.cos(y) + self.rp2 * sympy.sin(y) * sympy.I)
        e = (self.l * sympy.cos(x) + self.l * sympy.sin(x) * sympy.I) + (self.l2 * sympy.cos(y) + self.l2 * sympy.sin(y) * sympy.I)
        funcs1 = sympy.Matrix([d, e])
        args1 = sympy.Matrix([x, y])
        res1 = funcs1.jacobian(args1)
        return res1
    def 外力雅克比矩阵(self,x,y):
        return np.array([[-0.4*np.sin(x) + 0.4j*np.cos(x), -0.3*np.sin(y) + 0.3j*np.cos(y)], \
                       [-0.4*np.sin(x) + 0.4j*np.cos(x), -1.2*np.sin(y) + 1.2j*np.cos(y)]])
    def 重力矩阵(self,x,y):
        mg = np.array([-self.m1 * self.g * sympy.I, -self.m2 * self.g * sympy.I, -self.m3 * self.g * sympy.I])
        return mg
    def 约束矩阵(self,x,y):
        d = -(self.l * np.cos(x) / self.l2 * np.cos(y))
        dd=np.array([[1],[d]])
        return dd
    def 约束矩阵1(self,x,y):
        d = -(self.l * np.cos(x) / self.l2 * np.cos(y))
        return np.array([[d]])
    def 外力矩阵(self):
        f = np.array([-self.fx2 * 1j + self.fy2, self.fx3 * 1j])
        return f
    def 惯性质量对角阵(self):
        矩阵=np.array([self.m1,self.m2,self.m3])
        矩阵=np.diag(矩阵)
        return 矩阵
    def 转动惯量对角阵(self):
        矩阵 = np.mat([self.j1.self.j2])
        矩阵 = np.diag(矩阵)
        return 矩阵
    '''def 惯量矩阵乘法(self,x,y):
        惯量=self.约束矩阵(x,y).T*self.质心雅克比(x,y).T*self.惯性质量对角阵()*self.质心雅克比(x,y)*self.约束矩阵(x,y) \
            +self.约束矩阵1(x,y)*self.质心雅克比(x,y)*self.约束矩阵1(x,y).T
        惯量=np.inner(np.mat(self.质心雅克比(x, y)).H, self.惯性质量对角阵())
        惯量 =np.inner(惯量.H,np.mat(self.质心雅克比(x,y)))

        return 惯量'''

    '''def 位移方程(self, x):  # x0=角度1，x2位移
        return np.array([self.曲柄长度 * np.sin(self.thea) + self.连杆长度 * np.sin(x[0]), \
                         self.曲柄长度 * np.cos(self.thea) + self.连杆长度 * np.cos(x[0]) - x[1]])'''
    def 位移方程(self, x):  # x0=角度1，x2位移
        return np.array([self.曲柄长度 * np.sin(self.thea) + self.连杆长度 * np.sin(x[0]), \
                         self.曲柄长度 * np.cos(self.thea) + self.连杆长度 * np.cos(x[0]) - x[1]])
    def 解方程(self):
        sol3_fsolve = fsolve(self.位移方程, self.初始)
        return sol3_fsolve

    '''def 转动惯量计算(self, 曲柄角, 连杆角):
        转动惯量 = self.曲柄转动惯量 + self.曲柄质量 * self.曲柄质心 ** 2 + (self.连杆质量 + self.滑块质量) * self.曲柄长度 ** 2 + \
               (self.连杆质心)
        return 转动惯量'''
    def 等效力矩(self,滑块速度,曲柄绝对速度,):
        滑块功率=self.fx3*abs(滑块速度)
        曲柄功率=self.m1*self.g
        #等效力矩=滑块功率+连杆力功率+self.mm1+
        return
    def 曲柄连杆转动惯量表(self):
        数据 = pd.DataFrame()
        角度 = np.linspace(0, 4*np.pi, 1001)
        位移 = np.linspace(0, 4 * np.pi, 1001)
        角度2 = np.linspace(0, 4 * np.pi, 1001)
        时间 = np.linspace(0, 4 * np.pi / self.角速度, 1001)  # 时间
        书本转动惯量=np.linspace(0, 4 * np.pi, 1001)
        书本等效力矩=np.linspace(0, 4* np.pi, 1001)
        计算转动惯量 = np.linspace(0, 4 * np.pi, 1001)
        计算力矩矩阵 = np.linspace(0, 4 * np.pi, 1001)
        for i in range(len(角度)):
            self.thea = 角度[i]
            shuju = self.解方程()
            self.初始 = shuju
            位移[i] = shuju[1]
            角度2[i]=(shuju[0])
            书本转动惯量[i] = self.曲柄连杆机构书本转动惯量(float(角度[i]),float(shuju[0]))
            书本等效力矩[i]=self.力矩对比计算(float(角度[i]),float(角度2[i]))
            计算转动惯量[i]=float(self.等效惯量1(float(角度[i]),float(角度2[i])))
            计算力矩矩阵[i]=float(self.力矩矩阵(float(角度[i]),float(角度2[i])))
        # 最大位移=数据['位移'].max()
        滑块速度 = -np.gradient(位移, 时间, edge_order=2)
        滑块加速度 = np.gradient(滑块速度, 时间, edge_order=2)
        数据['位移'] = 位移
        print(位移)
        数据['计算力矩矩阵']=计算力矩矩阵
        最大位移 = 数据.loc[:, '位移'].max()
        数据['角度弧度'] = 角度
        数据['角度'] = (角度) / np.pi * 180
        数据['滑块速度'] = 滑块速度
        数据['滑块加速度'] = 滑块加速度
        数据['距离下死点位置'] = 最大位移-位移
        行程 = max(数据['距离下死点位置'])
        数据['角度2'] =角度2
        滑块速度=np.array(数据['滑块速度'])
        ES = 0.5 * self.m3* (滑块速度)**2; # 滑块动能，单位kg.m ^ 2
        EO2S = 0.5 * self.j1 * self.角速度**2; #% 偏心轴的动能
        #% 5 - 1.连杆的动能
        omega_L=-np.gradient(角度2,时间, edge_order=2)#角度的问题
        #omega_L=0
        VE = 滑块速度; #牵连速度
        LL=self.l2-self.rc2
        #LL=0.986
        VR = omega_L * LL;#相对速度
        VL = np.sqrt(VE**2+VR**2);
        #VL = np.sqrt(VE ** 2 + VR ** 2+abs(VE)*abs(VR)*np.cos(np.pi/2-abs(角度2)));
        EL = 0.5 * self.m2 * VL**2 + 0.5 * self.j2 * omega_L**2;#连杆的动能
        # 5 - 2.
        #等效转动惯量
        JO2V = 2/ self.角速度**2 * (ES+ EO2S+ EL)
        数据['滑块动能'] = ES
        数据['转动惯量']=JO2V
        数据['等效力矩']=书本等效力矩
        数据['计算等效惯量'] = 计算转动惯量
        fig, ax = plt.subplots()
        ax.set_title("角度-等效力矩计算")
        ax.plot(数据['角度'], 数据['等效力矩'])
        fig, ax = plt.subplots()
        ax.set_title("角度-角度2")
        ax.plot(数据['角度'], 数据['角度2'])
        fig, ax = plt.subplots()
        ax.set_title("角度-书本转动惯量")
        ax.plot(数据['角度'], 书本转动惯量)
        fig, ax = plt.subplots()
        ax.set_title("角度-转动惯量计算")
        ax.plot(数据['角度'], 数据['转动惯量'])
        fig, ax = plt.subplots()
        ax.set_title("角度-计算惯量计算")
        ax.plot(数据['角度'], 数据['计算等效惯量'])
        fig, ax = plt.subplots()
        ax.set_title("角度-计算力矩矩阵")
        ax.plot(数据['角度'], 数据['计算力矩矩阵'])
        '''fig, ax = plt.subplots()
        ax.set_title("角度-转动惯量计算")
        ax.plot(数据['角度'],数据['转动惯量'])
        fig, ax = plt.subplots()
        ax.set_title("角度-滑块速度")
        ax.plot(数据['角度'], 数据['滑块速度'])
        fig, ax = plt.subplots()
        ax.set_title("角度-位移")
        ax.plot(数据['角度'], 数据['位移'])
        fig, ax = plt.subplots()
        ax.set_title("角度-书本转动惯量")
        ax.plot(数据['角度'],书本转动惯量)
        fig, ax = plt.subplots()
        ax.set_title("角度-角度2角速度")
        ax.plot(数据['角度'],omega_L)
        fig, ax = plt.subplots()
        ax.set_title("角度-cos角度2")
        ax.plot(数据['角度'], np.cos(角度2))'''
        return 数据, 行程
    def 曲柄连杆机构书本转动惯量(self,角度1,角度2):
        约束=-(self.l * np.cos((角度1))/self.l2 * np.cos(角度2))
        减速字=-(self.m2*self.rc2+self.m3*self.l2)*self.l*np.cos(角度1-角度2)*(2*约束)
        #print(减速字)
        曲柄连杆机构书本转动惯量=self.j1+self.m1*(self.rc1**2)+(self.m2+self.m3)*self.l**2+(self.j2+self.m2*(self.rc2**2)+ \
        self.m3*(self.l2**2))*((约束)**2)-减速字
        return 曲柄连杆机构书本转动惯量

    '''-(self.m2 * \
      self.rc2 + \
      self.m3 * \
      self.l2) * \
    self.l * np.cos * (角度1 - 角度2) * 2 * 约束'''
    def 质心雅可比改(self,角度1,角度2):
        a, b, c, a1, b1, c1, x, y = sympy.symbols("a b c a1 bi c1 x y", real=True)
        a = (self.rc1 * sympy.cos(x))
        a1 = self.rc1 * sympy.sin(x)
        b = (self.l * sympy.cos(x)) + (self.rc2 * sympy.cos(y))
        b1 = self.l * sympy.sin(x) + self.rc2 * sympy.sin(y)
        c = (self.l * sympy.cos(x)) + (self.l2 * sympy.cos(y))
        c1 = self.l * sympy.sin(x) + self.l2 * sympy.sin(y)
        funcs = sympy.Matrix([a, a1, b, b1, c, c1])
        args = sympy.Matrix([x, y])
        res = funcs.jacobian(args)
        result = res.subs([(x,角度1),(y,角度2)])
        result=result.evalf()
        #g_func = lambdify((5,6),res)
        result=np.array(result)
        return result
    def 质心雅可比改1(self):
        a, b, c, a1, b1, c1, x, y = sympy.symbols("a b c a1 bi c1 x y", real=True)
        a = (self.rc1 * sympy.cos(x))
        a1 = self.rc1 * sympy.sin(x)
        b = (self.l * sympy.cos(x)) + (self.rc2 * sympy.cos(y))
        b1 = self.l * sympy.sin(x) + self.rc2 * sympy.sin(y)
        c = (self.l * sympy.cos(x)) + (self.l2 * sympy.cos(y))
        c1 = self.l * sympy.sin(x) + self.l2 * sympy.sin(y)
        funcs = sympy.Matrix([a, a1, b, b1, c, c1])
        args = sympy.Matrix([x, y])
        res = funcs.jacobian(args)
        return res
    def 改质心雅可比(self,角度1,角度2):
        x, y = sympy.symbols("x y", real=True)
        result = self.res.subs([(x,角度1),(y,角度2)])
        result=result.evalf()
        #g_func = lambdify((5,6),res)
        result=np.array(result)
        #print(result)
        return result
    def 质量对角阵(self):
        质量对角阵=np.diag([self.m1,self.m1,self.m2,self.m2,self.m3,self.m3])
        return 质量对角阵
    def 惯量对角阵(self):
        惯量对角阵 = np.diag([self.j1,self.j2])
        return 惯量对角阵
    def 等效惯量(self,角度1,角度2):
        等效惯量=self.约束矩阵(角度1,角度2).T.dot(self.质心雅可比改(角度1,角度2).T).dot(self.质量对角阵()).dot(self.质心雅可比改(角度1,角度2)).dot(self.约束矩阵(角度1,角度2)) \
        +self.约束矩阵(角度1,角度2).T.dot(self.惯量对角阵()).dot(self.约束矩阵(角度1,角度2))
        return 等效惯量
    def 等效惯量1(self,角度1,角度2):
        等效惯量=self.约束矩阵(角度1,角度2).T.dot(self.改质心雅可比(角度1,角度2).T).dot(self.质量对角阵()).dot(self.改质心雅可比(角度1,角度2)).dot(self.约束矩阵(角度1,角度2)) \
        +self.约束矩阵(角度1,角度2).T.dot(self.惯量对角阵()).dot(self.约束矩阵(角度1,角度2))
        return 等效惯量
    def 力的雅克比(self):
        a, b, a1, b1, x, y = sympy.symbols("a b a1 bi x y", real=True)
        a = (self.l * sympy.cos(x)+self.rp2 * sympy.cos(y))
        a1 = self.rc1 * sympy.sin(x)+self.rp2 * sympy.sin(y)
        b = (self.l * sympy.cos(x)) + (self.l2 * sympy.cos(y))
        b1 = self.l * sympy.sin(x) + self.l2 * sympy.sin(y)
        funcs = sympy.Matrix([a, a1, b, b1])
        args = sympy.Matrix([x, y])
        res = funcs.jacobian(args)
        print(res)
        return res
    def 改力雅可比(self,角度1,角度2):
        x, y = sympy.symbols("x y", real=True)
        result = self.力的雅克比1.subs([(x,角度1),(y,角度2)])
        result=result.evalf()
        #g_func = lambdify((5,6),res)
        result=np.array(result)
        #print(result)
        return result
    def 重力矩阵(self):
        return np.array([0,-self.m1*self.g,0,-self.m2*self.g,0,-self.m3*self.g])
    def 力矩阵(self):
        return np.array([-self.fx2,self.fy2,self.fx3,0])
    def 扭矩矩阵(self):
        return np.array([self.mm1,0])
    def 力矩矩阵(self,角度1,角度2):
        力矩=self.重力矩阵().dot(self.改质心雅可比(角度1,角度2)).dot(self.约束矩阵(角度1,角度2)) \
        +self.力矩阵().dot(self.改力雅可比(角度1,角度2)).dot(self.约束矩阵(角度1,角度2))+self.mm1
        return 力矩
if __name__=="__main__":
    x=5;y=10
    曲柄连杆=曲柄连杆测试()
    '''对比力矩=曲柄连杆.力矩对比计算(x,y)
    复数质心雅可比=np.mat(曲柄连杆.质心复数雅可比矩阵计算())
    外力雅可比=曲柄连杆.外力雅可比()
#    质心雅克比=np.mat(曲柄连杆.质心雅克比(x,y))
    重力矩阵=np.mat(曲柄连杆.重力矩阵(x,y))
    约束矩阵=曲柄连杆.约束矩阵(x,y)
    结果=复数质心雅可比@约束矩阵
    重力矩阵转置=重力矩阵@结果
    质量对角阵=曲柄连杆.惯性质量对角阵()
    索密克=曲柄连杆测试(rc1 = 1,l = 0.175,l2 = 1.231,rc2 =0.244,rp2 = 0.3,g = 9.8,m1 = 1, \
                 m2 = 7350,m3 = 37300,fx2 = 10,fy2=10,fx3 = 20,mm1 = 25,x = 5,y=10,j1=592,j2=1787)
    #曲柄滑块转动惯量表=索密克.曲柄连杆转动惯量表()
    书本惯量表=曲柄连杆.曲柄连杆转动惯量表()
    解方程=曲柄连杆.解方程()
    改变矩阵测试返回=曲柄连杆.质心雅可比改(x,y)
    print(改变矩阵测试返回)
    质量对角阵=曲柄连杆.质量对角阵()
    计算数=曲柄连杆.曲柄连杆机构书本转动惯量(x,y)'''
    #a=曲柄连杆.转换质心雅可比(x,y)
    #矩阵等效惯量=float(曲柄连杆.等效惯量1(x,y)[0,0])
    #矩阵等效惯量1=曲柄连杆.曲柄连杆转动惯量表()
    力矩矩阵=float(曲柄连杆.力矩矩阵(x,y))
    书本力矩=曲柄连杆.力矩对比计算(x,y)
    #print(矩阵等效惯量)


